#pragma once

// GHRのsigmoid延时函数
#include "sheriffos.h"

namespace os::math {
inline float sigmoid_table[64] = {
    0.00247262F, 0.00298989F, 0.00361498F, 0.00437018F, 0.00528231F, 0.0063836F, 0.00771271F, 0.00931596F,
    0.0112487F, 0.0135769F, 0.016379F, 0.0197479F, 0.0237929F, 0.0286423F, 0.0344452F, 0.0413737F,
    0.0496241F, 0.0594178F, 0.071F, 0.0846368F, 0.100609F, 0.119203F, 0.140696F, 0.165336F,
    0.193321F, 0.224768F, 0.259683F, 0.297937F, 0.339244F, 0.383152F, 0.429053F, 0.476208F,
    0.523792F, 0.570947F, 0.616848F, 0.660756F, 0.702063F, 0.740317F, 0.775232F, 0.806679F,
    0.834664F, 0.859304F, 0.880797F, 0.899391F, 0.915363F, 0.929F, 0.940582F, 0.950376F,
    0.958626F, 0.965555F, 0.971358F, 0.976207F, 0.980252F, 0.983621F, 0.986423F, 0.988751F,
    0.990684F, 0.992287F, 0.993616F, 0.994718F, 0.99563F, 0.996385F, 0.99701F, 0.997527F};

// 使用查找表进行Sigmoid计算
inline float sigmoid_with_lookup(float x) {
    // 对于极端值直接返回 0 或 1
    if (x > 6.0f) return 1.0f;
    if (x < -6.0f) return 0.0f;

    // 将输入映射到 [0, TABLE_SIZE-1] 范围
    auto const idx = static_cast<int>((x + 6.0f) * (63 - 1) / 12.0f);

    return sigmoid_table[idx];
}

template <typename T>
class SigmoidIter {
public:
    using SigmoidState = enum { Start, Iterating, Finished };

private:
    constexpr static float m_epsilon = 0.001F;
    T m_Start{};
    T m_Target{};
    float m_sigmoidVal{};

    units::time::second_t m_IterTime{};
    units::frequency::hertz_t m_TrigFrequency{};

    units::time::second_t m_TimeStride{};
    units::time::second_t m_LastTime{};
    units::dimensionless::scalar_t m_K{};
    T m_span{};

    SigmoidState m_State = SigmoidState::Finished;
    T m_Output{};

    void Iter() {
        switch (m_State) {
            case SigmoidState::Start:
                m_State = SigmoidState::Iterating;
                break;
            case SigmoidState::Iterating:
                if (m_sigmoidVal > 1.0F || (1 - m_sigmoidVal) < m_epsilon && fabs(m_span * (1 - m_sigmoidVal)) < 0.1F) {
                    m_sigmoidVal = 1.0F;
                    m_State = SigmoidState::Finished;
                } else if (m_sigmoidVal < 0.0F) {
                    m_sigmoidVal = m_epsilon;
                    m_State = SigmoidState::Start;
                } else {
                    m_sigmoidVal = m_sigmoidVal + m_K * m_sigmoidVal * (1.0F - m_sigmoidVal);
                }
                break;
            default:
                break;
        }
        m_Output = m_Start + m_span * m_sigmoidVal;
    }

public:
    SigmoidIter() = default;

    explicit SigmoidIter(units::frequency::hertz_t const& trigFrequency) {
        ResetFrequence(trigFrequency);
        m_sigmoidVal = m_epsilon;
    }

    void Set(T const& start, T const& target, units::time::second_t const& iterTime) {
        if (target == m_Target) {
            return;
        }

        m_Start = start;
        m_Target = target;
        m_span = m_Target - m_Start;

        m_IterTime = iterTime;
        m_K = m_TimeStride / m_IterTime * 14.F;
        m_LastTime = os::getDWTTime_u32();

        m_sigmoidVal = 0.009588;
        m_State = SigmoidState::Start;
    }

    void Set(T const& target, units::time::second_t const& iterTime) { Set(m_Output, target, iterTime); }

    void ResetFrequence(units::frequency::hertz_t const& trigFrequency) {
        m_TrigFrequency = trigFrequency;
        m_TimeStride = 1.F / m_TrigFrequency;
    }

    T Output() {
        units::time::microsecond_t now = os::getDWTTime_u32();
        if (now - m_LastTime >= m_TimeStride) {
            Iter();
            m_LastTime = now;
        }
        return m_Output;
    }

    T operator()() { return Output(); }
};
} // namespace os::math